//
// Created by fanghr on 18-3-26.
//

#include "../include/acclsen_driver.h"
#include "../../cssl2cpp/include/cssl2cpp.h"
#include <unistd.h>

namespace acclsen_driver {
    struct acclsen_t {
        cssl serial;
        struct {
            std::list<uint8_t> data;
            pthread_mutex_t mutex;
        } buffer;
        struct {
            acclsen_recv::time data;
            pthread_mutex_t mutex;
        } time;

        struct {
            acclsen_recv::acc data;
            pthread_mutex_t mutex;
        } acc;

        struct {
            acclsen_recv::angsp data;
            pthread_mutex_t mutex;
        } angsp;

        struct {
            acclsen_recv::ang data;
            pthread_mutex_t mutex;
        } ang;

        struct {
            acclsen_recv::magf data;
            pthread_mutex_t mutex;
        } magf;

        struct {
            acclsen_recv::port data;
            pthread_mutex_t mutex;
        } port;

        struct {
            acclsen_recv::prehei data;
            pthread_mutex_t mutex;
        } prehei;

        struct {
            acclsen_recv::latlon data;
            pthread_mutex_t mutex;
        } latlon;

        struct {
            acclsen_recv::grsp data;
            pthread_mutex_t mutex;
        } grsp;

        struct {
            acclsen_recv::foel data;
            pthread_mutex_t mutex;
        } foel;
        pthread_t parser, raw_getter;
    };

    class data_mutex_helper {
    public:
        explicit data_mutex_helper(pthread_mutex_t *mutex) {
            _mutex = mutex;
            pthread_mutex_lock(_mutex);
        }

        ~data_mutex_helper() {
            pthread_mutex_unlock(_mutex);
        }

    private:
        pthread_mutex_t *_mutex;
    };

    acclsen_t *acclsen_init(std::string path, int baud) {
        acclsen_t *ptr = (acclsen_t *) malloc(sizeof(acclsen_t));
        ptr->serial = cssl(path, nullptr, 0, baud);
        if (
                pthread_mutex_init(&ptr->buffer.mutex, nullptr) ||
                pthread_mutex_init(&ptr->time.mutex, nullptr) ||
                pthread_mutex_init(&ptr->acc.mutex, nullptr) ||
                pthread_mutex_init(&ptr->angsp.mutex, nullptr) ||
                pthread_mutex_init(&ptr->ang.mutex, nullptr) ||
                pthread_mutex_init(&ptr->magf.mutex, nullptr) ||
                pthread_mutex_init(&ptr->port.mutex, nullptr) ||
                pthread_mutex_init(&ptr->prehei.mutex, nullptr) ||
                pthread_mutex_init(&ptr->latlon.mutex, nullptr) ||
                pthread_mutex_init(&ptr->grsp.mutex, nullptr) ||
                pthread_mutex_init(&ptr->foel.mutex, nullptr) ||
                pthread_mutex_init(&ptr->buffer.mutex, nullptr)) {
            delete ptr;
            return nullptr;
        }

        ptr->serial.start();
        return ptr;
    }

    int acclsen_destory(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        if (acclsen_stop(obj))
            return -1;
        obj->serial.stop();
        free(obj);
        return 0;
    }

    static void *parser(void *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto ptr = (acclsen_t *) obj;
        while (true) {
            pthread_mutex_lock(&ptr->buffer.mutex);
            ptr->serial.getdata(ptr->buffer.data, 255);
            pthread_mutex_unlock(&ptr->buffer.mutex);
            sleep(1);
        }
    }

    static bool find_mark(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->buffer.mutex);
        while (obj->buffer.data.empty()) {
            if (obj->buffer.data.front() == 0x55) {
                return obj->buffer.data.size() >= 11;
            } else
                obj->buffer.data.pop_front();
            return false;
        }
    }

    static void *raw_getter(void *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto ptr = (acclsen_t *) obj;
        while (true) {
            pthread_mutex_lock(&ptr->buffer.mutex);
            if (find_mark(ptr)) {
                std::array<uint8_t, 11> tmp{0};
                for (auto i = 0; i < 11; i++) {
                    tmp[i] = ptr->buffer.data.front();
                    ptr->buffer.data.pop_front();
                }
                pthread_mutex_unlock(&ptr->buffer.mutex);
                switch (tmp[1]) {
                    case 0x50:
                        pthread_mutex_lock(&ptr->time.mutex);
                        ptr->time.data = acclsen_recv::time(tmp);
                        pthread_mutex_unlock(&ptr->time.mutex);
                        break;
                    case 0x51:
                        pthread_mutex_lock(&ptr->acc.mutex);
                        ptr->acc.data = acclsen_recv::acc(tmp);
                        pthread_mutex_unlock(&ptr->acc.mutex);
                        break;
                    case 0x52:
                        pthread_mutex_lock(&ptr->angsp.mutex);
                        ptr->angsp.data = acclsen_recv::angsp(tmp);
                        pthread_mutex_unlock(&ptr->angsp.mutex);
                        break;
                    case 0x53:
                        pthread_mutex_lock(&ptr->ang.mutex);
                        ptr->ang.data = acclsen_recv::ang(tmp);
                        pthread_mutex_unlock(&ptr->ang.mutex);
                        break;
                    case 0x54:
                        pthread_mutex_lock(&ptr->magf.mutex);
                        ptr->magf.data = acclsen_recv::magf(tmp);
                        pthread_mutex_unlock(&ptr->magf.mutex);
                        break;
                    case 0x55:
                        pthread_mutex_lock(&ptr->port.mutex);
                        ptr->port.data = acclsen_recv::port(tmp);
                        pthread_mutex_unlock(&ptr->port.mutex);
                        break;
                    case 0x56:
                        pthread_mutex_lock(&ptr->prehei.mutex);
                        ptr->prehei.data = acclsen_recv::prehei(tmp);
                        pthread_mutex_unlock(&ptr->prehei.mutex);
                        break;
                    case 0x57:
                        pthread_mutex_lock(&ptr->latlon.mutex);
                        ptr->latlon.data = acclsen_recv::latlon(tmp);
                        pthread_mutex_unlock(&ptr->latlon.mutex);
                        break;
                    case 0x58:
                        pthread_mutex_lock(&ptr->grsp.mutex);
                        ptr->grsp.data = acclsen_recv::grsp(tmp);
                        pthread_mutex_unlock(&ptr->grsp.mutex);
                        break;
                    case 0x59:
                        pthread_mutex_lock(&ptr->foel.mutex);
                        ptr->foel.data = acclsen_recv::foel(tmp);
                        pthread_mutex_unlock(&ptr->foel.mutex);
                        break;
                }
            }

            sleep(1);
        }
    }

    int acclsen_run(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        return pthread_create(&obj->raw_getter, nullptr, raw_getter, obj)
               || pthread_create(&obj->parser, nullptr, parser, obj) ? -1 : 0;
    }

    int acclsen_stop(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        return pthread_cancel(obj->parser) || pthread_cancel(obj->raw_getter) ? -1 : 0;
    }

    acclsen_recv::time get_time(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->time.mutex);
        return obj->time.data;
    }

    acclsen_recv::acc get_acc(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->acc.mutex);
        return obj->acc.data;
    }

    acclsen_recv::angsp get_angsp(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->angsp.mutex);
        return obj->angsp.data;
    }

    acclsen_recv::ang get_ang(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->ang.mutex);
        return obj->ang.data;
    }

    acclsen_recv::magf get_magf(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->magf.mutex);
        return obj->magf.data;
    }

    acclsen_recv::port get_port(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->port.mutex);
        return obj->port.data;
    }

    acclsen_recv::prehei get_prehei(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->prehei.mutex);
        return obj->prehei.data;
    }

    acclsen_recv::latlon get_latlon(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->latlon.mutex);
        return obj->latlon.data;
    }

    acclsen_recv::grsp get_grsp(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->grsp.mutex);
        return obj->grsp.data;
    }

    acclsen_recv::foel get_foel(acclsen_t *obj) {
        if (obj == nullptr)
            throw nullptr;
        auto lock = data_mutex_helper(&obj->foel.mutex);
        return obj->foel.data;
    }
}



